#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>
#include<bits/stdc++.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZINormal;
int frame_num_=0;
string frame_id_string_,pc_path_name_;
ros::Publisher pc_paint_pub_;

void pcd2bin(pcl::PointCloud<PointType>::Ptr cloud, string& out_file)
{
    cloud->width = 1;
    cloud->height = cloud->points.size();
    pcl::io::savePCDFileASCII<PointType>(out_file, *cloud);
}

pcl::PointCloud<PointType>::Ptr p_map_;
Eigen::Matrix4f lidar_pose_start_;
Eigen::Vector3d last_kf_rpy_(0,0,0),last_kf_t_(0,0,0);
bool judgeKeyframe(const Eigen::Vector3d& t_cur,const Eigen::Quaterniond& q_cur){
    Eigen::Vector3d test_rpy;
    double roll,pitch,yaw;
    tf::Quaternion q_temp(q_cur.x(),q_cur.y(),q_cur.z(),q_cur.w());
    tf::Matrix3x3(q_temp).getRPY(roll, pitch, yaw);
    test_rpy<<roll,pitch,yaw;
    if(abs(test_rpy[0] - last_kf_rpy_[0])<0.2&&
    abs(test_rpy[1] - last_kf_rpy_[1])<0.2&&
    abs(test_rpy[2] - last_kf_rpy_[2])<0.2&&
    (t_cur - last_kf_t_).norm()<1.0){
        return false;
    }else{
        last_kf_rpy_ = test_rpy;
        last_kf_t_ = t_cur;
        return true;
    }
}

void saveOdomEscur(const string& out_file,const Eigen::Vector3d& t,const Eigen::Quaterniond& q_rot){
    ofstream outfile(out_file.c_str(),ios::out|ios::binary|ios::app);
    if(!outfile) cout<<"tra error"<<endl;
    static bool is_first_line = true;
    if(is_first_line){
        outfile<<"index, timestamp, x, y, z, qx, qy, qz, qw"<<endl;
        is_first_line = false;
    }
    outfile<<frame_num_<<","<<frame_num_<<","
                <<t[0]<<","<<t[1]<<","<<t[2]<<","
                <<q_rot.x()<<","<<q_rot.y()<<","<<q_rot.z()<<","<<q_rot.w()<<std::endl;  //
    outfile.close();
}

void readPCDfile(const std::string finname, pcl::PointCloud<PointType>::Ptr p_pc)
{
	std::ifstream fin(finname);
	if (fin.bad()){
		std::cout << "打开文件失败！" << std::endl;
		return;
	}
 
	char s[11][1024]; //存储Header
	int Points_Num; //点数量
	for (int i = 0; i < 11; ++i){
		fin.getline(s[i], 1024);
		//POINTS xxx
		if (i == 9){
			std::string s9 = s[9], Points_Str;
			size_t pos = s9.find("POINTS");
			size_t size = s9.size();
			Points_Str = s9.substr(pos + 7, size);
			Points_Num = atoi(Points_Str.c_str());
		}
	}
    if(Points_Num>1e5)
        Points_Num = 1e5;
    p_pc->points.resize(Points_Num);
    for(int i=0;i<Points_Num;++i){
        fin >> p_pc->points[i].x >> p_pc->points[i].y>> p_pc->points[i].z>>
                 p_pc->points[i].intensity>> 
                 p_pc->points[i].normal_x>> p_pc->points[i].normal_y>>p_pc->points[i].normal_z>>
                 p_pc->points[i].curvature;
    }
    fin.close();
}

void saveMap(pcl::PointCloud<PointType>::Ptr cloud, string& out_file){
    ofstream outfile(out_file.c_str(),ios::out|ios::binary);
    if(!outfile) cout<<"tra error"<<endl;
     outfile<<"# .PCD v0.7 - Point Cloud Data file format"<<endl
                <<"VERSION 0.7"<<endl
                <<"FIELDS x y z intensity normal_x normal_y normal_z curvature"<<endl
                <<"SIZE 4 4 4 4 4 4 4 4"<<endl
                <<"TYPE F F F F F F F F"<<endl
                <<"COUNT 1 1 1 1 1 1 1 1"<<endl
                <<"WIDTH 1"<<endl
                <<"HEIGHT "<<cloud->points.size()<<endl
                <<"VIEWPOINT 0 0 0 1 0 0 0"<<endl
                <<"POINTS "<<cloud->points.size()<<endl
                <<"DATA ascii"<<endl;
    for(int i=0;i<cloud->points.size();++i){
        if(cloud->points[i].intensity!=0){
            outfile<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<" "
            <<16515072<<" "
            <<0<<" "<<0<<" "<<0<<" "
            <<0<<std::endl;  //
        }else{
            outfile<<cloud->points[i].x<<" "<<cloud->points[i].y<<" "<<cloud->points[i].z<<" "
            <<0<<" "
            <<0<<" "<<0<<" "<<0<<" "
            <<0<<std::endl;  //
        }
    }
    outfile.close();
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;

    pc_paint_pub_ = nh.advertise<sensor_msgs::PointCloud2>("pc_paint",10);

    p_map_.reset(new pcl::PointCloud<PointType>);
    bool is_first_ = true;
    string master_data_path = "/home/st/ubuntu_data/dataset/keti/envalute/2022_02/testing/trajectory/master.txt";
    ifstream read_master;
    read_master.open(master_data_path);
    string s_line;
    vector<string> pcd_path_v_;
    while(getline(read_master,s_line)){
        istringstream sin(s_line);
        vector<string> data_line;
        string temp;
        for(int i=0;i<11;++i){
            sin>>temp;
            data_line.push_back(temp);
        } 
        pcd_path_v_.push_back(data_line[1]);
    }
    read_master.close();

    read_master.open(master_data_path);
    while(getline(read_master,s_line)){
        istringstream sin(s_line);
        vector<string> data_line;
        string temp;
        for(int i=0;i<11;++i){
            sin>>temp;
            data_line.push_back(temp);
        }
     
        pcl::PointCloud<PointType>::Ptr p_pc(new pcl::PointCloud<PointType>);
        readPCDfile(pcd_path_v_[stoi(data_line[0])],p_pc);
        // pcl::VoxelGrid<PointType> dw;
        // dw.setInputCloud(p_pc);
        // dw.setLeafSize(0.1,size,size);
        // dw.filter(*pc);
    
        Eigen::Vector3d t_cur( stod(data_line[3]),stod(data_line[4]), stod(data_line[5]));
        Eigen::Quaterniond q_cur( stod(data_line[6]),stod(data_line[7]),stod(data_line[8]),stod(data_line[9]));

        frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                        "%06d", frame_num_));
        cout<<frame_id_string_<<endl;
        string main_path = "/home/st/ubuntu_data/dataset/keti/envalute/2022_02/ESCUR";
        string re_name="pcds";
        if(is_first_){
            string scan_path=main_path+"/"+re_name+"/"+frame_id_string_+".pcd";
            pcd2bin(p_pc,scan_path);

            lidar_pose_start_.setIdentity();
            string pose_path = main_path+"/"+"poses_lidar2body.csv";
            saveOdomEscur(pose_path,Eigen::Vector3d(0,0,0),Eigen::Quaterniond(1,0,0,0));

            lidar_pose_start_(0,3) = t_cur[0];
            lidar_pose_start_(1,3) = t_cur[1];
            lidar_pose_start_(2,3) = t_cur[2];
            Eigen::Quaternionf q_temp(q_cur.w(),q_cur.x(),q_cur.y(),q_cur.z());
            lidar_pose_start_.block<3,3>(0,0)=q_temp.toRotationMatrix();
            *p_map_ += *p_pc;
            is_first_ = false;
            frame_num_++;
            continue;
        }

        Eigen::Matrix4f T_mas(Eigen::Matrix4f::Identity());
        T_mas(0,3) = t_cur[0];
        T_mas(1,3) = t_cur[1];
        T_mas(2,3) = t_cur[2];
        Eigen::Quaternionf q_temp(q_cur.w(),q_cur.x(),q_cur.y(),q_cur.z());
        T_mas.block<3,3>(0,0)=q_temp.toRotationMatrix();
        T_mas = (lidar_pose_start_.inverse()*T_mas).eval();
        t_cur<<T_mas(0,3),T_mas(1,3),T_mas(2,3);
        Eigen::Matrix3d mat_temp;
        for(int i=0;i<3;++i){
            for(int j=0;j<3;++j){
                mat_temp(i,j) = T_mas(i,j);
            }
        }
        q_cur = Eigen::Quaterniond(mat_temp);

        if(judgeKeyframe(t_cur,q_cur)){
            string scan_path=main_path+"/"+re_name+"/"+frame_id_string_+".pcd";
            pcd2bin(p_pc,scan_path);
            frame_num_++;

            pcl::transformPointCloud(*p_pc,*p_pc,T_mas);
            *p_map_ += *p_pc;

            string pose_path = main_path+"/"+"poses_lidar2body.csv";
            saveOdomEscur(pose_path,t_cur,q_cur);
        }

        //publish
        sensor_msgs::PointCloud2 tempCloud;
        pcl::toROSMsg(*p_map_, tempCloud);
        tempCloud.header.stamp = ros::Time::now();
        tempCloud.header.frame_id = "map";
        pc_paint_pub_.publish(tempCloud);

        // if(frame_num_>=317)
        if(stoi(data_line[0])==pcd_path_v_.size()-1){
            // string pose_path = main_path+"/"+"gt_map.pcd";
            // // saveMap(p_map_,pose_path);
            // pcd2bin(p_map_,pose_path);

            pcl::VoxelGrid<PointType> dw;
            dw.setInputCloud(p_map_);
            dw.setLeafSize(0.05,0.05,0.05);
            dw.filter(*p_map_);
            string pose_path = main_path+"/"+"dense_global_map.pcd";
            pcd2bin(p_map_,pose_path);
            break;
        }
        
        // cv::Mat m_t(640,480,CV_8UC3,cv::Scalar(0,0,0));
        // cv::imshow("test",m_t);
        // cv::waitKey(0);
    }

    return 0;
}
